﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace NestedParticleFilter
{
    public class Drive
    {
        private const double DistanceBtweenWheels = 267; //millimeters mm
        public const double StraightOmega = 0.0005;
        public const double NintyDegree = 90.0;
        /// <summary>
        /// Returns the rotational velocity based on the left and right wheel in degree/second
        /// </summary>
        /// <param name="leftVelocity">left velocity mm/second</param>
        /// <param name="rightVelocity">right velocity mm/second</param>
        /// <returns>rotational velocity (degree/second) </returns>
        public static double Omega(double rightVelocity, double leftVelocity)
        {
            // ω = (vl-vr)/distance between wheels
            if (leftVelocity == rightVelocity)
                return StraightOmega;
            double o = (leftVelocity - rightVelocity) / DistanceBtweenWheels;
            o = UnitConversion.RadianToDegree(o);
            return (o);
        }

        /// <summary>
        /// Velocity of the robot based on the left and right wheel speed. (mm/sec)
        /// </summary>
        /// <param name="leftWheel">left wheel velocity (mm/sec)</param>
        /// <param name="rightWheel">right wheel velocity (mm/sec)</param>
        /// <returns>the velocity of the robot (mm/sec) </returns>
        public static double Velocity(double leftWheel, double rightWheel)
        {
            if (leftWheel == -rightWheel)
                return 0.0001; // rotting in place
            // return leftWheel;
            return ((leftWheel + rightWheel) / 2.0);
        }

    }
}
